#include "communication_task.h"

#include "bsp_uart.h"
#include "cmsis_os.h"
#include "communication.h"

#include "gimbal_task.h"

#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t communication_high_water;
#endif

// 任务相关时间
#define COMMUNICATION_TASK_INIT_TIME 100
#define COMMUNICATION_TASK_TIME_MS 2

gimbal_control_t* gimbal_contral = NULL;

void communication_task(void const * pvParameters)
{

    // gimbal_contral = get_gimbal_control();

    Usart1Init();
    // 空闲一段时间
    vTaskDelay(COMMUNICATION_TASK_INIT_TIME);
    while (1) {

        //下面是串口通信协议没用Vofa+
        // DataPack("Hello World", 13, 0);
        // UartSendTxMessage(&huart1, (uint8_t *)(&BOARD_TX_DATA), sizeof(BOARD_TX_DATA), 100);
        // DataUnpack();

        //下面使用Vofa+ (JustFloat)协议
        DataVofaPack(
            gimbal_contral->gimbal_pitch_motor.absolute_angle, 
            gimbal_contral->gimbal_pitch_motor.absolute_angle_set,
            gimbal_contral->gimbal_yaw_motor.absolute_angle,
            gimbal_contral->gimbal_yaw_motor.absolute_angle_set
        );

        UartSendTxMessage(&huart1, (uint8_t *)(&VOFA_FRAME_SEND), sizeof(VOFA_FRAME_SEND), 100);

        // 系统延时
        vTaskDelay(COMMUNICATION_TASK_TIME_MS);

#if INCLUDE_uxTaskGetStackHighWaterMark
        communication_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}
